
#include "MSE_PID.h"
#include "MSE_DBUS.h"

uint8_t MSE_F_MOTOR_PID_COMMON_INIT(struct MSE_MOTOR_Typedef* MOTOR , uint8_t MOD , float* PID_S , float* PID_P)
{
    switch (MOD)
    {
        case MSE_DF_PID_DOUBLE:
        {
            MOTOR->PID_P.P       = PID_P[0];
            MOTOR->PID_P.I       = PID_P[1];
            MOTOR->PID_P.D       = PID_P[2];
            MOTOR->PID_P.I_Lit   = PID_P[3];
            MOTOR->PID_P.All_Lit = PID_P[4];

            MOTOR->PID_S.P       = PID_S[0];
            MOTOR->PID_S.I       = PID_S[1];
            MOTOR->PID_S.D       = PID_S[2];
            MOTOR->PID_S.I_Lit   = PID_S[3];
            MOTOR->PID_S.All_Lit = PID_S[4];

            MOTOR->PID_INIT = MSE_DF_READY;

            return MSE_DF_READY;
        } 
        case MSE_DF_PID_SINGLE:
        {
            MOTOR->PID_S.P       = PID_S[0];
            MOTOR->PID_S.I       = PID_S[1];
            MOTOR->PID_S.D       = PID_S[2];
            MOTOR->PID_S.I_Lit   = PID_S[3];
            MOTOR->PID_S.All_Lit = PID_S[4];

            MOTOR->PID_INIT = MSE_DF_READY;

            return MSE_DF_READY;
        } 
        default:
			
            return MSE_DF_ERROR;
    }
}

void MSE_F_MOTOR_PID_CURRENT_INIT(struct MSE_MOTOR_Typedef* MOTOR , float* PID_C)
{
    MOTOR->PID_C.P       = PID_C[0];
    MOTOR->PID_C.I       = PID_C[1];
    MOTOR->PID_C.D       = PID_C[2];
    MOTOR->PID_C.I_Lit   = PID_C[3];
    MOTOR->PID_C.All_Lit = PID_C[4];
}

uint8_t MSE_F_MOTOR_PID_HEAD_INIT(void)
{
	/******************************抬升前馈赋值********************************/
	float C_LIFT_L[3]					= WHW_DF_FFC_DATA_HEAD_LIFT_L;
	float C_LIFT_R[3]					= WHW_DF_FFC_DATA_HEAD_LIFT_R;
	
	Feedforward_Init(&FFC_ALL.LIFT_L,4000,C_LIFT_L,0.5f,2,2);
	Feedforward_Init(&FFC_ALL.LIFT_R,4000,C_LIFT_R,0.5f,2,2);
	
	/******************************抬升PID双环赋值********************************/
	float MSE_HEAD_ANGLE_LIFT[5] 		= MSE_DF_PID_DATA_HEAD_LIFT_P;
    float MSE_HEAD_SPEED_LIFT[5]  		= MSE_DF_PID_DATA_HEAD_LIFT_S;
	
	MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_LIFT_L,MSE_DF_PID_DOUBLE,MSE_HEAD_SPEED_LIFT,MSE_HEAD_ANGLE_LIFT);
	MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_LIFT_R,MSE_DF_PID_DOUBLE,MSE_HEAD_SPEED_LIFT,MSE_HEAD_ANGLE_LIFT);

	/******************************前伸前馈赋值********************************/
	float C_HORIZONTAL_L[3]					= WHW_DF_FFC_DATA_HEAD_HORIZONTAL_L;
	float C_HORIZONTAL_R[3]					= WHW_DF_FFC_DATA_HEAD_HORIZONTAL_R;
	
	Feedforward_Init(&FFC_ALL.HORIZONTAL_L,4000,C_HORIZONTAL_L,0.5f,2,2);
	Feedforward_Init(&FFC_ALL.HORIZONTAL_R,4000,C_HORIZONTAL_R,0.5f,2,2);
	
	/******************************前伸PID双环赋值********************************/
	float MSE_HEAD_ANGLE_HORIZONTAL[5]	   		= MSE_DF_PID_DATA_HEAD_HORIZONTAL_P;
	float MSE_HEAD_SPEED_HORIZONTAL[5]	   		= MSE_DF_PID_DATA_HEAD_HORIZONTAL_S;
	
	MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_HORIZONTAL_L,MSE_DF_PID_DOUBLE,MSE_HEAD_SPEED_HORIZONTAL,MSE_HEAD_ANGLE_HORIZONTAL);
	MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_HORIZONTAL_R,MSE_DF_PID_DOUBLE,MSE_HEAD_SPEED_HORIZONTAL,MSE_HEAD_ANGLE_HORIZONTAL);
	
	/******************************横移前馈赋值********************************/
	float C_SWAY[3]							= WHW_DF_FFC_DATA_HEAD_SWAY;
	
	Feedforward_Init(&FFC_ALL.SWAY,4000,C_SWAY,0.5f,2,2);
	
	/******************************横移PID双环赋值********************************/
	float WHW_MIDDLE_ANGLE_SWAY[5] 	= MSE_DF_PID_DATA_HEAD_SWAY_P;
	float WHW_MIDDLE_SPEED_SWAY[5] 	= MSE_DF_PID_DATA_HEAD_SWAY_S;
	
	MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_SWAY,MSE_DF_PID_DOUBLE,WHW_MIDDLE_SPEED_SWAY,WHW_MIDDLE_ANGLE_SWAY);

    /******************************Yaw轴前馈赋值********************************/
    float C_Yaw[3]					= WHW_DF_FFC_DATA_HEAD_Yaw;

    Feedforward_Init(&FFC_ALL.Yaw,4000,C_Yaw,0.5f,2,2);

    /******************************Yaw轴PID赋值********************************/
    float WHW_MIDDLE_ANGLE_Yaw[5] 	= MSE_DF_PID_DATA_HEAD_Yaw_P;
	float WHW_MIDDLE_SPEED_Yaw[5] 	= MSE_DF_PID_DATA_HEAD_Yaw_S;

    MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_Yaw,MSE_DF_PID_DOUBLE,WHW_MIDDLE_SPEED_Yaw,WHW_MIDDLE_ANGLE_Yaw);

    /******************************差动关节PID赋值********************************/
	float CUP_PUMP_L[3]					= WHW_DF_FFC_DATA_HEAD_CUP_PUMP_L;
	float CUP_PUMP_R[3]					= WHW_DF_FFC_DATA_HEAD_CUP_PUMP_R;
	
	Feedforward_Init(&FFC_ALL.CUP_PUMP_L,4000,C_Yaw,0.5f,2,2);
	Feedforward_Init(&FFC_ALL.CUP_PUMP_R,4000,C_Yaw,0.5f,2,2);
	
    float MSE_HEAD_ANGLE_CUP_PUMP_P[5] 		= MSE_DF_PID_DATA_HEAD_CUP_PUMP_P;
    float MSE_HEAD_SPEED_CIP_PUMP_P[5]  	= MSE_DF_PID_DATA_HEAD_CUP_PUMP_S;
	
	MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_L_CUP_PUMP  ,MSE_DF_PID_DOUBLE,MSE_HEAD_ANGLE_CUP_PUMP_P,MSE_HEAD_SPEED_CIP_PUMP_P);
	MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_R_CUP_PUMP  ,MSE_DF_PID_DOUBLE,MSE_HEAD_ANGLE_CUP_PUMP_P,MSE_HEAD_SPEED_CIP_PUMP_P);
	
	/******************************底盘PID赋值********************************/
	float MSE_MOTOR_BOTTOM_3508_PID_SPEED[5]   = MSE_DF_PID_DATA_BOTTOM_3508_SPEED;
    float MSE_MOTOR_BOTTOM_3508_PID_CURRENT[5] = MSE_DF_PID_DATA_BOTTOM_3508_CURENT;

    MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_BOTTOM_3508_1 , MSE_DF_PID_SINGLE , MSE_MOTOR_BOTTOM_3508_PID_SPEED , 0);
    MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_BOTTOM_3508_2 , MSE_DF_PID_SINGLE , MSE_MOTOR_BOTTOM_3508_PID_SPEED , 0);
    MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_BOTTOM_3508_3 , MSE_DF_PID_SINGLE , MSE_MOTOR_BOTTOM_3508_PID_SPEED , 0);
    MSE_F_MOTOR_PID_COMMON_INIT(&MSE_V_MOTOR_BOTTOM_3508_4 , MSE_DF_PID_SINGLE , MSE_MOTOR_BOTTOM_3508_PID_SPEED , 0);

    MSE_F_MOTOR_PID_CURRENT_INIT(&MSE_V_MOTOR_BOTTOM_3508_1 , MSE_MOTOR_BOTTOM_3508_PID_CURRENT);
    MSE_F_MOTOR_PID_CURRENT_INIT(&MSE_V_MOTOR_BOTTOM_3508_2 , MSE_MOTOR_BOTTOM_3508_PID_CURRENT);
    MSE_F_MOTOR_PID_CURRENT_INIT(&MSE_V_MOTOR_BOTTOM_3508_3 , MSE_MOTOR_BOTTOM_3508_PID_CURRENT);
    MSE_F_MOTOR_PID_CURRENT_INIT(&MSE_V_MOTOR_BOTTOM_3508_4 , MSE_MOTOR_BOTTOM_3508_PID_CURRENT);
	
	
	return MSE_DF_READY;
}



void MSE_F_MOTOR_CHSISS_3508RM(struct MSE_MOTOR_Typedef* MOTOR)
{
    MOTOR->DATA.dt = DWT_GetDeltaT((void *)&MOTOR->DATA.DWT_CNT);

    MOTOR->PID_S.Error[MSE_DF_NOW] = MOTOR->DATA.Aim - MOTOR->DATA.Speed_now;
    /*比例输出*/
    MOTOR->PID_S.P_out = (MOTOR->PID_S.Error[MSE_DF_NOW] * MOTOR->PID_S.P);
    /*微分输出*/
    MOTOR->PID_S.D_out = (MOTOR->PID_S.Error[MSE_DF_NOW] - MOTOR->PID_S.Error[MSE_DF_LAST]) / MOTOR->DATA.dt * MOTOR->PID_S.D;
    /*积分输出*/
    MOTOR->PID_S.I_out += (MOTOR->PID_S.Error[MSE_DF_NOW] * MOTOR->PID_S.I);
    MOTOR->PID_S.I_out += MOTOR->PID_S.D_out;
    /*积分限幅*/
    MOTOR->PID_S.I_out = MSE_F_MATH_Limit_float(MOTOR->PID_S.I_Lit , -MOTOR->PID_S.I_Lit , MOTOR->PID_S.I_out);
    /*数据迭代*/
    MOTOR->PID_S.Error[MSE_DF_LAST] = MOTOR->PID_S.Error[MSE_DF_NOW];
    /*速度环总输出*/
    MOTOR->PID_S.All_out = (MOTOR->PID_S.P_out + MOTOR->PID_S.I_out);
    /*总输出限幅*/
    MOTOR->PID_S.All_out = MSE_F_MATH_Limit_float(MOTOR->PID_S.All_Lit , -MOTOR->PID_S.All_Lit , MOTOR->PID_S.All_out);

    /*电流环*/
    MOTOR->PID_C.Error[MSE_DF_NOW] = MOTOR->PID_S.All_out - MOTOR->DATA.current;
    /*比例输出*/
    MOTOR->PID_C.P_out = (MOTOR->PID_C.Error[MSE_DF_NOW] * MOTOR->PID_C.P);
    /*微分输出*/
    MOTOR->PID_C.D_out = (MOTOR->PID_C.Error[MSE_DF_NOW] - MOTOR->PID_C.Error[MSE_DF_LAST]) / MOTOR->DATA.dt * MOTOR->PID_C.D;
    /*积分输出*/
    MOTOR->PID_C.I_out += (MOTOR->PID_C.Error[MSE_DF_NOW] * MOTOR->PID_C.I);
    /*积分限幅*/
    MOTOR->PID_C.I_out = MSE_F_MATH_Limit_float(MOTOR->PID_C.I_Lit , -MOTOR->PID_C.I_Lit , MOTOR->PID_C.I_out);
    /*数据迭代*/
    MOTOR->PID_C.Error[MSE_DF_LAST] = MOTOR->PID_C.Error[MSE_DF_NOW];
    /*速度环总输出*/
    MOTOR->PID_C.All_out = (MOTOR->PID_C.P_out + MOTOR->PID_C.I_out);
    /*总输出限幅*/
    MOTOR->PID_C.All_out = MSE_F_MATH_Limit_float(MOTOR->PID_C.All_Lit , -MOTOR->PID_C.All_Lit , MOTOR->PID_C.All_out);

}

void MSE_F_MOTOR_PID_TWO_3508RM(struct MSE_MOTOR_Typedef* MOTOR)
{
    MOTOR->DATA.dt = DWT_GetDeltaT((void *)&MOTOR->DATA.DWT_CNT);

    MOTOR->PID_P.Error[MSE_DF_NOW] = MOTOR->DATA.Aim - MOTOR->DATA.Angle_Infinite;
    /*比例输出*/
    MOTOR->PID_P.P_out = (MOTOR->PID_P.Error[MSE_DF_NOW] * MOTOR->PID_P.P);
    /*微分输出*/
    MOTOR->PID_P.D_out = (MOTOR->PID_P.Error[MSE_DF_NOW] - MOTOR->PID_P.Error[MSE_DF_LAST]) / MOTOR->DATA.dt * MOTOR->PID_P.D;
    /*积分输出*/
    MOTOR->PID_P.I_out += (MOTOR->PID_P.Error[MSE_DF_NOW] * MOTOR->PID_P.I);
    MOTOR->PID_P.I_out += MOTOR->PID_P.D_out;
    /*积分限幅*/
    MOTOR->PID_P.I_out = MSE_F_MATH_Limit_float(MOTOR->PID_P.I_Lit , -MOTOR->PID_P.I_Lit , MOTOR->PID_P.I_out);
    /*数据迭代*/
    MOTOR->PID_P.Error[MSE_DF_LAST] = MOTOR->PID_P.Error[MSE_DF_NOW];
    /*角度环总输出*/
    MOTOR->PID_P.All_out = (MOTOR->PID_P.P_out + MOTOR->PID_P.I_out);
    /*总输出限幅*/
    MOTOR->PID_P.All_out = MSE_F_MATH_Limit_float(MOTOR->PID_P.All_Lit , -MOTOR->PID_P.All_Lit , MOTOR->PID_P.All_out);

    MOTOR->PID_S.Error[MSE_DF_NOW] = MOTOR->PID_P.All_out - MOTOR->DATA.Speed_now;

    /*比例输出*/
    MOTOR->PID_S.P_out = (MOTOR->PID_S.Error[MSE_DF_NOW] * MOTOR->PID_S.P);
    /*微分输出*/
    MOTOR->PID_S.D_out = (MOTOR->PID_S.Error[MSE_DF_NOW] - MOTOR->PID_S.Error[MSE_DF_LAST]) / MOTOR->DATA.dt * MOTOR->PID_S.D;
    /*积分输出*/
    MOTOR->PID_S.I_out += (MOTOR->PID_S.Error[MSE_DF_NOW] * MOTOR->PID_S.I);
    MOTOR->PID_S.I_out += MOTOR->PID_S.D_out;
    /*积分限幅*/
    MOTOR->PID_S.I_out = MSE_F_MATH_Limit_float(MOTOR->PID_S.I_Lit , -MOTOR->PID_S.I_Lit , MOTOR->PID_S.I_out);
    /*数据迭代*/
    MOTOR->PID_S.Error[MSE_DF_LAST] = MOTOR->PID_S.Error[MSE_DF_NOW];
    /*速度环总输出*/
    MOTOR->PID_S.All_out = (MOTOR->PID_S.P_out + MOTOR->PID_S.I_out);
    /*总输出限幅*/
    MOTOR->PID_S.All_out = MSE_F_MATH_Limit_float(MOTOR->PID_S.All_Lit , -MOTOR->PID_S.All_Lit , MOTOR->PID_S.All_out);
}

void MSE_F_MOTOR_PID_TWO_2006RM(struct MSE_MOTOR_Typedef* MOTOR)
{
    MOTOR->DATA.dt = DWT_GetDeltaT((void *)&MOTOR->DATA.DWT_CNT);

    MOTOR->PID_P.Error[MSE_DF_NOW] = MOTOR->DATA.Aim - MOTOR->DATA.Angle_Infinite;
    /*比例输出*/
    MOTOR->PID_P.P_out = (MOTOR->PID_P.Error[MSE_DF_NOW] * MOTOR->PID_P.P);
    /*微分输出*/
    MOTOR->PID_P.D_out = (MOTOR->PID_P.Error[MSE_DF_NOW] - MOTOR->PID_P.Error[MSE_DF_LAST]) / MOTOR->DATA.dt * MOTOR->PID_P.D;
    /*积分输出*/
    MOTOR->PID_P.I_out += (MOTOR->PID_P.Error[MSE_DF_NOW] * MOTOR->PID_P.I);
    MOTOR->PID_P.I_out += MOTOR->PID_P.D_out;
    /*积分限幅*/
    MOTOR->PID_P.I_out = MSE_F_MATH_Limit_float(MOTOR->PID_P.I_Lit , -MOTOR->PID_P.I_Lit , MOTOR->PID_P.I_out);
    /*数据迭代*/
    MOTOR->PID_P.Error[MSE_DF_LAST] = MOTOR->PID_P.Error[MSE_DF_NOW];
    /*角度环总输出*/
    MOTOR->PID_P.All_out = (MOTOR->PID_P.P_out + MOTOR->PID_P.I_out);
    /*总输出限幅*/
    MOTOR->PID_P.All_out = MSE_F_MATH_Limit_float(MOTOR->PID_P.All_Lit , -MOTOR->PID_P.All_Lit , MOTOR->PID_P.All_out);

    MOTOR->PID_S.Error[MSE_DF_NOW] = MOTOR->PID_P.All_out - MOTOR->DATA.Speed_now;

    /*比例输出*/
    MOTOR->PID_S.P_out = (MOTOR->PID_S.Error[MSE_DF_NOW] * MOTOR->PID_S.P);
    /*微分输出*/
    MOTOR->PID_S.D_out = (MOTOR->PID_S.Error[MSE_DF_NOW] - MOTOR->PID_S.Error[MSE_DF_LAST]) / MOTOR->DATA.dt * MOTOR->PID_S.D;
    /*积分输出*/
    MOTOR->PID_S.I_out += (MOTOR->PID_S.Error[MSE_DF_NOW] * MOTOR->PID_S.I);
    MOTOR->PID_S.I_out += MOTOR->PID_S.D_out;
    /*积分限幅*/
    MOTOR->PID_S.I_out = MSE_F_MATH_Limit_float(MOTOR->PID_S.I_Lit , -MOTOR->PID_S.I_Lit , MOTOR->PID_S.I_out);
    /*数据迭代*/
    MOTOR->PID_S.Error[MSE_DF_LAST] = MOTOR->PID_S.Error[MSE_DF_NOW];
    /*速度环总输出*/
    MOTOR->PID_S.All_out = (MOTOR->PID_S.P_out + MOTOR->PID_S.I_out);
    /*总输出限幅*/
    MOTOR->PID_S.All_out = MSE_F_MATH_Limit_float(MOTOR->PID_S.All_Lit , -MOTOR->PID_S.All_Lit , MOTOR->PID_S.All_out);
}


void MPU_MOTOR_PID_ONE_2006RM(struct MSE_MOTOR_Typedef* MOTOR,float Aim_Angle,float Date_MPU_Angle)
{
    MOTOR->DATA.dt = DWT_GetDeltaT((void *)&MOTOR->DATA.DWT_CNT);

    MOTOR->PID_S.Error[MSE_DF_NOW] = Aim_Angle - Date_MPU_Angle;
    /*比例输出*/
    MOTOR->PID_S.P_out = (MOTOR->PID_S.Error[MSE_DF_NOW] * MOTOR->PID_S.P);
    /*微分输出*/
    MOTOR->PID_S.D_out = (MOTOR->PID_S.Error[MSE_DF_NOW] - MOTOR->PID_S.Error[MSE_DF_LAST]) / MOTOR->DATA.dt * MOTOR->PID_S.D;
    /*积分输出*/
    MOTOR->PID_S.I_out += (MOTOR->PID_S.Error[MSE_DF_NOW] * MOTOR->PID_S.I);
    MOTOR->PID_S.I_out += MOTOR->PID_S.D_out;
    /*积分限幅*/
    MOTOR->PID_S.I_out = MSE_F_MATH_Limit_float(MOTOR->PID_S.I_Lit , -MOTOR->PID_S.I_Lit , MOTOR->PID_S.I_out);
    /*数据迭代*/
    MOTOR->PID_S.Error[MSE_DF_LAST] = MOTOR->PID_S.Error[MSE_DF_NOW];
    /*角度环总输出*/
    MOTOR->PID_S.All_out = (MOTOR->PID_S.P_out + MOTOR->PID_S.I_out);
    /*总输出限幅*/
    MOTOR->PID_S.All_out = MSE_F_MATH_Limit_float(MOTOR->PID_S.All_Lit , -MOTOR->PID_S.All_Lit , MOTOR->PID_S.All_out);
}

void MSE_F_MOTOR_PID_TWO_6020RM(struct MSE_MOTOR_Typedef* MOTOR)
{
    MOTOR->DATA.dt = DWT_GetDeltaT((void *)&MOTOR->DATA.DWT_CNT);

    MOTOR->PID_P.Error[MSE_DF_NOW] = MOTOR->DATA.Aim - MOTOR->DATA.Angle_Infinite;
    /*比例输出*/
    MOTOR->PID_P.P_out = (MOTOR->PID_P.Error[MSE_DF_NOW] * MOTOR->PID_P.P);
    /*微分输出*/
    MOTOR->PID_P.D_out = (MOTOR->PID_P.Error[MSE_DF_NOW] - MOTOR->PID_P.Error[MSE_DF_LAST]) / MOTOR->DATA.dt * MOTOR->PID_P.D;
    /*积分输出*/
    MOTOR->PID_P.I_out += (MOTOR->PID_P.Error[MSE_DF_NOW] * MOTOR->PID_P.I);
    MOTOR->PID_P.I_out += MOTOR->PID_P.D_out;
    /*积分限幅*/
    MOTOR->PID_P.I_out = MSE_F_MATH_Limit_float(MOTOR->PID_P.I_Lit , -MOTOR->PID_P.I_Lit , MOTOR->PID_P.I_out);
    /*数据迭代*/
    MOTOR->PID_P.Error[MSE_DF_LAST] = MOTOR->PID_P.Error[MSE_DF_NOW];
    /*角度环总输出*/
    MOTOR->PID_P.All_out = (MOTOR->PID_P.P_out + MOTOR->PID_P.I_out);
    /*总输出限幅*/
    MOTOR->PID_P.All_out = MSE_F_MATH_Limit_float(MOTOR->PID_P.All_Lit , -MOTOR->PID_P.All_Lit , MOTOR->PID_P.All_out);

    MOTOR->PID_S.Error[MSE_DF_NOW] = MOTOR->PID_P.All_out - MOTOR->DATA.Speed_now;

    /*比例输出*/
    MOTOR->PID_S.P_out = (MOTOR->PID_S.Error[MSE_DF_NOW] * MOTOR->PID_S.P);
    /*微分输出*/
    MOTOR->PID_S.D_out = (MOTOR->PID_S.Error[MSE_DF_NOW] - MOTOR->PID_S.Error[MSE_DF_LAST]) / MOTOR->DATA.dt * MOTOR->PID_S.D;
    /*积分输出*/
    MOTOR->PID_S.I_out += (MOTOR->PID_S.Error[MSE_DF_NOW] * MOTOR->PID_S.I);
    MOTOR->PID_S.I_out += MOTOR->PID_S.D_out;
    /*积分限幅*/
    MOTOR->PID_S.I_out = MSE_F_MATH_Limit_float(MOTOR->PID_S.I_Lit , -MOTOR->PID_S.I_Lit , MOTOR->PID_S.I_out);
    /*数据迭代*/
    MOTOR->PID_S.Error[MSE_DF_LAST] = MOTOR->PID_S.Error[MSE_DF_NOW];
    /*速度环总输出*/
    MOTOR->PID_S.All_out = (MOTOR->PID_S.P_out + MOTOR->PID_S.I_out);
    /*总输出限幅*/
    MOTOR->PID_S.All_out = MSE_F_MATH_Limit_float(MOTOR->PID_S.All_Lit , -MOTOR->PID_S.All_Lit , MOTOR->PID_S.All_out);
}

